/*
 * planner_queue.h
 *
 * Copyright 2011 Pieter Agten
 *
 * This file is part of Yarf.
 *
 * Yarf is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Yarf is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Yarf.  If not, see <http://www.gnu.org/licenses/>.
 */

/**
 * @file   planner_queue.h
 * @author Pieter Agten (pieter.agten@gmail.com)
 * @date   23 sep 2011
 * @brief  The planning queue stores blocks prepared by the planner, until they
 *         are executed by the block handler.
 *
 * The planning queue is implemented as a circular buffer. Its size is
 * determined by the \a PLANNER_QUEUE_SIZE configuration parameter. The queue
 * is implemented almost completely in the \a planner_queue.h header file, so
 * the compiler will be able to optimize it aggressively.
 */

#ifndef PLANNER_QUEUE_H
#define PLANNER_QUEUE_H

#include "yarf.h"
#include "block.h"

#include <stdbool.h>
#include <stddef.h>

/**
 * Array for storing the blocks in the planning queue.
 */
extern block_t q[PLANNER_QUEUE_SIZE];

/**
 * Index of the head of the queue, which is the location where the next block
 * to enter the queue will be added.
 */
extern volatile uint8_t q_head;

/**
 * Index of the tail of the queue, which is the location of the next block to
 * executed by the block handler.
 */
extern volatile uint8_t q_tail;




/**
 * Returns the index of the block added after the block specified by the given
 * index.
 *
 * @param block_index the index for which to return the next index
 * @return The next index after the given index.
 */
inline uint8_t 
plan_q_next_index(uint8_t block_index)
{
  block_index += 1;
  if (block_index == PLANNER_QUEUE_SIZE) {
    block_index = 0;
  }
  return block_index;
}


/**
 * Returns the index of the block added before the block specified by the given
 * block_index.
 *
 * @param block_index the index for which to return the previous index
 * @return The previous index before the given index.
 */
inline uint8_t
plan_q_prev_index(uint8_t block_index) {
  if (block_index == 0) {
    block_index = PLANNER_QUEUE_SIZE;
  }
  block_index -= 1;
  return block_index;
}

/**
 * Returns whether the planning queue is full.
 * 
 * @return true if the planning queue if full and false otherwise.
 */
inline bool
plan_q_full(void)
{
  return (plan_q_next_index(q_head) == q_tail);
}

/**
 * Returns whether the planning queue is empty.
 * 
 * @return true if the planning queue if empty and false otherwise.
 */
inline bool
plan_q_empty(void)
{
  return (q_head == q_tail);
}

/**
 * Returns the number of items in the planning queue.
 *
 * @return The number of items in the planning queue.
 */
inline uint8_t
plan_q_size(void)
{
  uint8_t size = q_head - q_tail;
  return size >= 0 ? size : (PLANNER_QUEUE_SIZE - size);
}

/**
 * Returns the block at the specified index in the planning queue.
 *
 * @param index the index in the planning queue of the block to return
 * @return The block at the specified index in the planning queue.
 */
inline block_t *
plan_q_get(uint8_t index)
{
  return &q[index];
}


/**
 * Returns a pointer to the block at the head of the planning queue. This block
 * is considered to empty, so the pointer can be used to add a new block to the
 * queue.  If the queue is full, this function returns NULL.
 *
 * @return A pointer to the block at the head of the planning queue or NULL if
 *         the queue is full.
 */
inline block_t *
plan_q_head(void)
{
  if (plan_q_full()) {
    return NULL;
  }
  return &q[q_head];
}

/**
 * Returns the index of the head of the queue, i.e. the index of an empty 
 * position where a new block can be written. The caller is responsible for
 * checking that the queue is not full before writing to the returned location.
 *
 * @return The index of the head of the queue (i.e. where a new block can be 
 *         added).
 */
inline uint8_t
plan_q_head_index(void)
{
  return q_head;
}

/**
 * Shifts the head pointer of the planning queue, registering a new block that
 * was added to the queue.
 */
inline void
plan_q_shift_head(void)
{
  if (!plan_q_full()) {
    q_head = plan_q_next_index(q_head);
  }
}

/**
 * Returns a pointer to the block at the tail of the planning queue. The tail
 * block is the oldest block in the queue. If the queue is empty, this function
 * returns NULL.
 *
 * @return A pointer to the block at the tail of the planning queue or NULL if
 *         the queue is empty.
 */
inline block_t *
plan_q_tail(void)
{
  if (plan_q_empty()) { 
    return NULL;
  }
  return &q[q_tail];
}


/**
 * Returns the index of the tail of the queue, i.e. the index of the oldest
 * block in the queue. The caller is responsible for checking that the queue is
 * not empty when using the returned index.
 *
 * @return The index of the tail of the queue (i.e. the index of the oldest
 *         block in the queue).
 */
inline uint8_t
plan_q_tail_index(void)
{
  return q_tail;
}

/**
 * Discards the oldest block in the queue, making room for a new block to be 
 * added.
 */
inline void
plan_q_discard_tail(void)
{
  if (!plan_q_empty()) {
    q_tail = plan_q_next_index(q_tail);
  }
}

/**
 * Clears the planning queue.
 */
inline void
plan_q_clear(void)
{
  q_head = 0;
  q_tail = 0;
}

/**
 * Initializes the planning queue.
 */
inline void
plan_q_init(void)
{
  plan_q_clear();
}

#endif //PLANNER_QUEUE_H
